#include "XmlParser.h"

void XmlParser::extractNodeData(string node_name, string task_name,
		map<string, string>& params) throw (const char*) {

	// find XML file's node matching ROS node's parameters
	xml_node node_to_parse = findNode(node_name);

	// After expected node was found, search for node "Task" with attribute name = task_name (input parameter)
	for (pugi::xml_node_iterator it = node_to_parse.begin();
			it != node_to_parse.end(); ++it) {

		if (strcmp(it->name(), "Task") != 0
				|| strcmp(it->attribute("name").value(), task_name.c_str())	!= 0) {

			continue;
		}

		xml_node param_node = it->child("Params");
		readParams(param_node, params);

	} // for task
}

void XmlParser::extractNodeData(string node_name, map<string, string>& params)
		throw (const char*) {

	// find XML file's node matching ROS node's parameters
	xml_node node_to_parse = findNode(node_name);

	// if task_name is empty
	readParams(node_to_parse.child("Params"), params);
}

/**
 * Finds node in XML file matching indicated ROS node
 * @param node_name ROS node name to find in XML file
 * @return node in XML file holding parameters for indicated ROS node
 * @throws error message if XML file does not hold any node matching indicated ROS node
 */
xml_node XmlParser::findNode(string node_name) throw(const char*) {

	xml_document xml_doc;

	// open the document
	xml_parse_result result = xml_doc.load_file("/home/komodo/ros_workspace/project/komodoprj/params.xml");

	// check opening errors
	if (strcmp("No error", result.description()) != 0) {
		cout << "Load result: " << result.description() << endl;
	}

	// temporary traversing node
	xml_node curr_node;

	// Search for node "RosNode" with attribute name = node_name (input parameter)
	for (curr_node = xml_doc.child("RosNode"); curr_node;
			curr_node = curr_node.next_sibling("RosNode")) {

		// check if found needed node
		if (strcmp(curr_node.attribute("name").value(), node_name.c_str()) == 0) {
			// found
			return curr_node;
		}
	}

	// could not find node in XML file
	throw ERROR_MSG;
}

void XmlParser::readParams(xml_node param_node, map<string, string>& params) {

	// search for node "Params"
	for (pugi::xml_node_iterator params_it = param_node.begin();
			params_it != param_node.end(); ++params_it) {

		string key = params_it->name();
		string value = params_it->child_value();

		// remove spaces, tabs and \n
		trimAll(key);
		trimAll(value);

		// add to map
		params[key] = value;
	} // for params
}

void XmlParser::trimAll(std::string &s) {

	if (s.size() == 0) {
		return;
	}

	int val = 0;
	for (unsigned int cur = 0; cur < s.size(); cur++) {
		if (s[cur] != ' ' && s[cur] != '\t' && s[cur] != '\n') {
			s[val] = s[cur];
			val++;
		}
	}
	s.resize(val);
}
